package edu.ntnu.controller;

import java.util.ArrayList;
import java.util.List;
import edu.wsu.KheperaSimulator.RobotController;
import edu.wsu.KheperaSimulator.KSGripperStates;

/*
 * Moving across maze, record way and objects
 */
public class Explore extends State {
	
	private List<Point> toExplore;
	boolean finished;
	Point last;

	public Explore(RobotController newRobot) {
		super(newRobot);
		System.out.println("Starting exploring");
		toExplore = new ArrayList<Point>();
		finished = false;
	}
	
	/**
	 * checks crossroad and marks if there are any unexplored ways
	 * @return true if there is still any way to scout, false otherwise
	 */
	public boolean scoutCrossroad() {
		
		// stuck protection
		if (mapBuilder.getPosition().equals(last)) {
			System.err.println("Stuck at " + mapBuilder.getPosition());
			while (toExplore.contains(last)) {
				toExplore.remove(last);
			}
			if(!toExplore.isEmpty() && !driver.moveTo(toExplore.get(toExplore.size()-1))) {
				toExplore.remove(toExplore.size()-1);
			}
			return false;
		}
		last = new Point(mapBuilder.getPosition());
		
		toExplore.remove(mapBuilder.getPosition());
		
		// check left, front, right and back sides
		List<Integer> possibleTurns = new ArrayList<Integer>();
		if(robot.getDistanceValue(0) < 100 && robot.getDistanceValue(1) < 1000 && !mapBuilder.isExplored(90)){
			possibleTurns.add(90);
		}
		if(robot.getDistanceValue(1) < 1000 && robot.getDistanceValue(2) < 100 && robot.getDistanceValue(3) < 100 &&
		   robot.getDistanceValue(4) < 1000 && !mapBuilder.isExplored(0)){
			possibleTurns.add(0);
		}
		if(robot.getDistanceValue(5) < 100 && robot.getDistanceValue(4) < 1000 && !mapBuilder.isExplored(-90)){
			possibleTurns.add(-90);
		}
		if(robot.getDistanceValue(6) < 100 && robot.getDistanceValue(7) < 100 && !mapBuilder.isExplored(180)){
			possibleTurns.add(180);
		}
		
		if (possibleTurns.size() > 1) {
			toExplore.add(mapBuilder.getPosition());
		}
		if (possibleTurns.size() > 0) {
			driver.turn(possibleTurns.get(0), Movement.SPEED);
			return true;
		} else {
			if(!toExplore.isEmpty() && !driver.moveTo(toExplore.get(toExplore.size()-1))) {
				System.err.println("Failed to move to last unexplored crossroad");
				toExplore.remove(toExplore.size()-1);
			}
			return false;
		}
	}

	/**
	 * moves from crossroad
	 */
	public void moveFromCrossroad() {
		if (driver.canMoveForward()) {
			driver.moveForward(270, Movement.SPEED);
		}
	}
	
	/**
	 * moves to next crossroad
	 */
	public void moveToCrossroad() {
		while (driver.canMoveForward() && robot.getDistanceValue(0) > 10 && robot.getDistanceValue(5) > 10) {
			driver.moveForward(Movement.SPEED, Movement.SPEED);
		}
		driver.moveForward(144, Movement.SPEED);
	}


	/**
	 * checks whether the object in front of robot is light and record it into the map
	 * @return true if there is light next to the robot, false otherwise
	 */
	public boolean recordLight() {
		for(int i = 1; i < 5; i++) {
			if(robot.getLightValue(i) < 100) {
				mapBuilder.record(Map.LIGHT);
				System.out.println("Ligth was found");
				return true;
			}
		}
		return false;
	}
	
	/**
	 * checks whether the object in front of robot is ball and record it into the map
	 * @return	true if ball was recorded, false otherwise
	 */
	public boolean recordBall() {
		
		boolean result = false;
			
		// there is only wall
		if (robot.getDistanceValue(2) > 950 && robot.getDistanceValue(3) > 950) {
			return result;
		}
		
		// move and turn to ball
		driver.moveForward(10, 1, true);
		int turned = driver.turnToBall();
		
		// check if there is ball
		robot.setGripperState(KSGripperStates.GRIP_OPEN);
		robot.setArmState(KSGripperStates.ARM_DOWN);
		robot.sleep(50);

		// if there is a ball record it into the map
		if (robot.isObjectPresent()) {
			mapBuilder.record(Map.BALL);
			result = true;
			System.out.println("Ball was found");
		}

		// turn back
		robot.setArmState(KSGripperStates.ARM_UP);
		driver.turn(turned * -1, 1);
		
		return result;
	}
	
	@Override
	public boolean doAction() {
		
		if(finished && toExplore.isEmpty()) {
			return true;
		}
		
		if (!scoutCrossroad()) {
			finished = true;
		} else {
			finished = false;
			moveFromCrossroad();
			moveToCrossroad();
			if(!driver.canMoveForward()) {
				if(!recordLight()) {
					recordBall();
				}
			}
		}
		return false;
	}
	
}
